/*
 * File:   CAN_I.c
 * Author: aravelomanantsoa@gmail.com
 */

#include <libpcan.h>
#include <stdlib.h>
#include <fcntl.h>
#include "CAN_I.h"
#include "Utils.h"

/* send remote frame */
int sendRemoteFrame(HANDLE h, DWORD ID) {
	TPCANMsg msg;

	/* build CAN message */
	msg.ID = ID;
	msg.LEN = 0;
	msg.MSGTYPE = MSGTYPE_RTR;

	if (CAN_Write(h, &msg) != CAN_ERR_OK) {
		return CANNOT_SEND_REMOTE_FRAME;
	}

	return 0;
}

/* send data frame */
int sendDataFrame(HANDLE h, DWORD ID, BYTE LEN, BYTE data[]) {
	TPCANMsg msg;
	int i = 0;

	/* build CAN message */
	msg.ID = ID;
	msg.LEN = LEN;
	msg.MSGTYPE = MSGTYPE_STANDARD;
	for (i = 0; i < LEN; i++) {
		msg.DATA[i] = data[i];
	}

	if (CAN_Write(h, &msg) != CAN_ERR_OK) {
		return CANNOT_SEND_DATA_FRAME;
	}

	return 0;
}

/* read CAN message */
static int readFrame(HANDLE h, DWORD ID, TPCANMsg *msg) {
	bool not_found = TRUE;

	while (not_found) {
		/* read CAN message */
		if (CAN_Read(h, msg) != CAN_ERR_OK) {
			return CANNOT_READ_CAN_MSG;
		}
		if (msg->ID == ID) {
			not_found = FALSE;
		}
	}

	return 0;
}

int updateVitesse(VITESSE v) {
	BYTE data[1] = { 0 };

	/* open CAN device */HANDLE h = LINUX_CAN_Open(CAN_DEVICE_NODE, O_WRONLY);
	if (h == NULL) {
		return CANNOT_OPEN_CAN_DEVICE; /* failed opening CAN device */
	}

	switch (v) {
	case V1:
		data[0] = 0;
		break;
	case V2:
		data[0] = 10;
		break;
	case V3:
		data[0] = 20;
		break;
	case V4:
		data[0] = 30;
		break;
	case V5:
		data[0] = 40;
		break;
	case V6:
		data[0] = 50;
		break;
	case V7:
		data[0] = 60;
		break;
	case V8:
		data[0] = 70;
		break;
	case V9:
		data[0] = 80;
		break;
	case V10:
		data[0] = 90;
		break;
	default:
		data[0] = 0;
		break;
	}

	if (sendDataFrame(h, VITESSE_ID, 1, data) == CANNOT_SEND_DATA_FRAME) {
		CAN_Close(h);

		return CANNOT_SEND_DATA_FRAME;
	}

	CAN_Close(h);

	return 0;
}

int updateDirection(DIRECTION d) {
	BYTE data[1] = { 0 };

	/* open CAN device */HANDLE h = LINUX_CAN_Open(CAN_DEVICE_NODE, O_WRONLY);
	if (h == NULL) {
		return CANNOT_OPEN_CAN_DEVICE; /* failed opening CAN device */
	}

	switch (d) {
	case D1:
		data[0] = 0;
		break;
	case D2:
		data[0] = 5;
		break;
	case D3:
		data[0] = 10;
		break;
	case D4:
		data[0] = 15;
		break;
	case D5:
		data[0] = 20;
		break;
	case D6:
		data[0] = 25;
		break;
	case D7:
		data[0] = 30;
		break;
	case D8:
		data[0] = 35;
		break;
	case D9:
		data[0] = 40;
		break;
	case D10:
		data[0] = 45;
		break;
	case D11:
		data[0] = 50;
		break;
	case D12:
		data[0] = 55;
		break;
	case D13:
		data[0] = 60;
		break;
	case D14:
		data[0] = 65;
		break;
	case D15:
		data[0] = 70;
		break;
	case D16:
		data[0] = 75;
		break;
	case D17:
		data[0] = 80;
		break;
	case D18:
		data[0] = 85;
		break;
	case D19:
		data[0] = 90;
		break;
	case D20:
		data[0] = 95;
		break;
	default:
		data[0] = 0;
		break;
	}

	if (sendDataFrame(h, DIRECTION_ID, 1, data) == CANNOT_SEND_DATA_FRAME) {
		CAN_Close(h);

		return CANNOT_SEND_DATA_FRAME;
	}

	CAN_Close(h);

	return 0;
}

int freinage() {

	/* open CAN device */
	HANDLE h = LINUX_CAN_Open(CAN_DEVICE_NODE, O_WRONLY);
	if (h == NULL) {
		return CANNOT_OPEN_CAN_DEVICE; /* failed opening CAN device */
	}

	if (sendDataFrame(h, FREINAGE_ID, 0, NULL) == CANNOT_SEND_DATA_FRAME) {
		CAN_Close(h);

		return CANNOT_SEND_DATA_FRAME;
	}

	CAN_Close(h);

	return 0;
}

int fin_de_course(FIN_DE_COURSE f, BYTE *fin_de_sourse) {
	TPCANMsg msg;
	DWORD ID;

	/* open CAN device */HANDLE h = LINUX_CAN_Open(CAN_DEVICE_NODE, O_WRONLY);
	if (h == NULL) {
		return CANNOT_OPEN_CAN_DEVICE; /* failed opening CAN device */
	}

	switch (f) {
	case FIN_DE_COURSE_GAUCHE:
		ID = FIN_DE_COURSE_GAUCHE_ID;
		break;
	case FIN_DE_COURSE_DROITE:
		ID = FIN_DE_COURSE_DROITE_ID;
		break;
	default:
		ID = FIN_DE_COURSE_GAUCHE_ID;
		break;
	}

	if (readFrame(h, ID, &msg) == CANNOT_READ_CAN_MSG) {
		CAN_Close(h);

		return CANNOT_READ_CAN_MSG;
	}

	*fin_de_sourse = msg.DATA[0];

	CAN_Close(h);

	return 0;
}

int readBatterie(int *batterie) {
	TPCANMsg msg;
	float b = 0.0;

	/* open CAN device */HANDLE h = LINUX_CAN_Open(CAN_DEVICE_NODE, O_WRONLY);
	if (h == NULL) {
		return CANNOT_OPEN_CAN_DEVICE; /* failed opening CAN device */
	}

	if (readFrame(h, BATTERIE_ID, &msg) == CANNOT_READ_CAN_MSG) {
		CAN_Close(h);

		return CANNOT_READ_CAN_MSG;
	}

	b = byteArray2int(msg.DATA[0], msg.DATA[1]) * (100.0 / 1023.0);

	*batterie = (int) b;

	CAN_Close(h);

	return 0;
}

int readVitesse(int vitesse[]) {
	TPCANMsg msg;

	/* open CAN device */HANDLE h = LINUX_CAN_Open(CAN_DEVICE_NODE, O_WRONLY);
	if (h == NULL) {
		return CANNOT_OPEN_CAN_DEVICE; /* failed opening CAN device */
	}

	/* Read V1 */
	if (readFrame(h, V1_ID, &msg) == CANNOT_READ_CAN_MSG) {
		CAN_Close(h);

		return CANNOT_READ_CAN_MSG;
	}
	vitesse[0] = byteArray2int(msg.DATA[0], msg.DATA[1]);

	/* Read V2 */
	if (readFrame(h, V2_ID, &msg) == CANNOT_READ_CAN_MSG) {
		CAN_Close(h);

		return CANNOT_READ_CAN_MSG;
	}
	vitesse[1] = byteArray2int(msg.DATA[0], msg.DATA[1]);

	/* Read V3 */
	if (readFrame(h, V3_ID, &msg) == CANNOT_READ_CAN_MSG) {
		CAN_Close(h);

		return CANNOT_READ_CAN_MSG;
	}
	vitesse[2] = byteArray2int(msg.DATA[0], msg.DATA[1]);

	/* Read V4 */
	if (readFrame(h, V4_ID, &msg) == CANNOT_READ_CAN_MSG) {
		CAN_Close(h);

		return CANNOT_READ_CAN_MSG;
	}
	vitesse[3] = byteArray2int(msg.DATA[0], msg.DATA[1]);

	/* close CAN device */
	CAN_Close(h);

	return 0;
}
